/***************************************************
 * 定义一些常用的数学量和函数
 * */
#ifndef CPP_MY_FUNCTIONS_H
#define CPP_MY_FUNCTIONS_H

#include <cmath>
#include <string>
#include <fstream>
#include <iostream>
//容器
#include <vector>

namespace myNumpy {
using namespace std;
const double kPI = 3.14159265358979323846; //定义pi
template<class T>
inline T rad2deg(T rad) { return rad * 180.0 / kPI; }
template<class T>
inline T deg2rad(T deg) { return deg * kPI / 180.0; }


/** 将角度的范围限制在[-pi,pi]
 *  @parm angle [rad]
 *  @return angle in radian in [-pi,pi]
 */
template<class T>
inline T normalizeAngle(T angle) {
    while (angle > kPI)
        angle -= 2.0 * kPI;
    while (angle < -kPI)
        angle += 2.0 * kPI;
    return angle;
}

/**
 * 读取txt中的路径点
 * @param address 路径点在电脑中的存储路径
 * @return Eigen::ArrayXf 返回一个数组，数组中存放着路径点的信息
 * */
template<class T>
inline vector<T> readTxt(const string &address) {
    //1.创建流对象
    fstream read_stream1;
    //创建变量的储存器
    vector<T> vector1;
    vector1.reserve(70000);
    //2.打开文件并且判断是否成功
    read_stream1.open(address, ios::in);
    if (!read_stream1.is_open()) {
        cout << "文件打开失败" << endl;
    }
    //3.读数据
    string buf;
    while (getline(read_stream1, buf)) {
        T a = stof(buf); //将字符串转成double类型的值.
        vector1.push_back(a);
    }
    //4.关闭文件
    read_stream1.close();
    return vector1;
}
template<class T>
inline T lowerPass(T x_n, T y_n_1) {
    static T k = 0.1;
    return k * x_n + (1 - k) * y_n_1;
}
};// namespace myNumpy


#endif //CPP_MY_FUNCTIONS_H
